#include "includes.h"
  
void PID_init(pid_t *pid)
{
  pid->SetCurrent=0;
  pid->ActualCurrent=0;
  pid->err=0;
  pid->err_last=0;
  pid->Increment=0;
  pid->integral=0;
  pid->Kp=2;
  pid->Ki=1;
  pid->umax=400;
  pid->umin=-200;
}
  
int16_t PID_realize(int16_t Current, int16_t ActualCurrent)
{  
  uint8_t index;
  uint16_t i;
  pid_t pid;
  PID_init(&pid);
  pid.SetCurrent = Current;
  pid.ActualCurrent = ActualCurrent;
  pid.err=pid.SetCurrent-pid.ActualCurrent;
  if(pid.ActualCurrent>pid.umax)
  { 
    if(pid.err<0)
      i = 0-pid.err;
      if(i>200){  
        index=0;  
      }
      else{
        index=1;  
        if(pid.err<0){  
          pid.integral+=pid.err;  
        }  
      }  
      }
    else if(pid.ActualCurrent<pid.umin){  
      if(pid.err<0)
        i = 0-pid.err; 
      if(i>200){  
        index=0;  
      }
      else{  
        index=1; 
        
        if(pid.err>0){  
          pid.integral+=pid.err;  
        }  
      }  
    }
    else{  
      if(pid.err<0)
        i = 0-pid.err; 
      if(i>200){  
        index=0;  
      }
      else{  
        index=1;  
        pid.integral+=pid.err;  
      }  
    }  
  pid.Increment=pid.Kp*(pid.err-pid.err_last)+index*pid.Ki*pid.integral;
  pid.err_last=pid.err;
  return pid.Increment;
}  
